/********************************************************************
* Thank friend GG_ruler for his help 
* Thank friend GG_ruler for his help 
* Thank friend GG_ruler for his help 
* Thank friend GG_ruler for his help 
* @brief		    
    This file is used to write programs related to interrupt tasks
* @author           Chuyan

********************************************************************/
#include "Pit.h"

PID Servo_gyro_pid, Gyro_pid, Servo_Angle_pid;

GPS_target GPS_Data;


float Gyro_z, Gyro_exp_yaw, Gyro_com, Servo_GYRO_Z; // useless
float Aoa_tar, Aoa_tar_last, Aoa_out, Aoa_former;
uint8 MOTION_State;
float dis_last, Aoa_former_last;  //previous distance
// hall encode
float encoder, V_old, get_speed_sum, meter_second, hall_dis;
//Servo_CASCADE control串级控制
float Yaw;//新建变量防止刚复制完后，立刻被WIT串口替换
float ggyro_z;//新建变量防止刚复制完后，立刻被WIT串口替换
float gps_Yaw;//角度环记录读取gps时的yaw
float detaYaw;//角度外环yaw角度差

/**********************************************************************************************


*********************************************************************************************/
//-------------------------------------------------------------------------------------------------------------------
// @brief		    Steering gear calculation function
// @param		    -200  200 20 
// @return		             - 
// @since		    
// Sample usage: 
//gps_tau1201.direction                 A:  Current nose azimuth
//GPS_Data.former_i_dir[GPS_Data.i]     B:  I-1 points to the azimuth of I
//target_direction                      C:  The azimuth between the current car body and point I
//
/*
舵机控制 5ms中断    05/17
若 有新的GPS信号
对GPS信息进行处理 计算得到每次所需角速度
对GPS数据进行pid处理 输出得到舵机值
无 GPS信号时
对所需转角进行pid处理 输出得到舵机值
*/
//-------------------------------------------------------------------------------------------------------------------
void Servo_Timer()
{   
        //5ms角度外环（为角度环的外环）
        Yaw = Angle_limiter(WIT.WITangle_Yaw);//+-180//新建变量防止刚复制完后，立刻被WIT串口替换
        if(gps_Yaw>150 && Yaw<-150) detaYaw = (Yaw+360)-gps_Yaw;
        else if(gps_Yaw<-150 && Yaw>150) detaYaw = Yaw-(gps_Yaw+360);
        else detaYaw = Yaw - gps_Yaw;
        Esp_ang = Loca_PID(&Servo_Angle_pid, Aoa_tar, detaYaw);
#if 0 //整定角速度内环gyro_z
        Esp_ang = 0;
#endif
        //5ms角速度内环（为角度环的内环）
        // Due to the update target point, the steering engine misjudgment phenomenon
        // So do a processing
        if (0 == flag_updata)
            Servo_PWM = Loca_PID(&Servo_gyro_pid, Esp_ang, WIT.WITangle_gyro_z);//Esp_ang
        else
            flag_updata = 0;

    Servo_PWM = limit_ab(Servo_PWM, SERVO_LOWER_m, SERVO_UPPER_m);  // limiting
}

//-------------------------------------------------------------------------------------------------------------------
// @brief		    PID init
// @param		    PID_Ser sturcture
// @return          
// @since		    
// Sample usage:	PID_Iint(&Gyro_pid, &Servo_gyro_pid, &Servo_Angle_pid);
//-------------------------------------------------------------------------------------------------------------------
void PID_Iint(PID* PID_Gyro, PID* PID_Servo_Gyro, PID* PID_Servo_Angle)
{
    PID_Servo_Angle->P = 3.5f;            
    PID_Servo_Angle->I = 0.f;             
    PID_Servo_Angle->D = 2.5f;      
    
    PID_Servo_Gyro->P = 1.f;
    PID_Servo_Gyro->I = 0;               
    PID_Servo_Gyro->D = 0.25f;            
    PID_Servo_Gyro->mid_value   = SERVO_MEDIAN;   
    
    PID_Gyro->P = 15.f;               
    PID_Gyro->I = 0.f;             
    PID_Gyro->D = 4.f;                
    PID_Gyro->mid_value         = SERVO_MEDIAN;      
}

//-------------------------------------------------------------------------------------------------------------------
// @brief		    Exercise analysis====To be improved  
// @param		    GPS structure
// @return		    
// explain          to be improved
// Sample usage:	Exercise_analysis( &GPS_Data);
/*
运动分析函数 100ms中断 06/30

更新目标点条件
下一目标点存在
数据状态：接收到新GPS数据状态               //新GPS数据状态 仅当接收并解析新GPS数据时置1， 仅于更新目标点时 置0

若 当前目标距离小于5

目标点下移
重新计算GPS数据

更新参数
*/
//-------------------------------------------------------------------------------------------------------------------
void Exercise_analysis(GPS_target* GPSData)
{   
    // Only enter the automatic analysis state, do the judgment of whether to reach the target point
    if (RC.STATE == RC_auto)
    {  
        // The next target point exists
        if(GPSData->lon[GPSData->i+1] != 0 )
        {
            // If there's data from the new GPS
            if(0X01 ==gps_tau1201.GPS_new_data_exe ) 
            {
                // the current distance is less than 2                 
                if( target_distance < 5 )
                {
                    // update Status of new GPS data
                    gps_tau1201.GPS_new_data_exe = 0; 
                    
                    flag_updata = 1;
                    
                    // The target point moves down
                    GPSData->i++;
                    
                    // GPS data is updated again due to target point update
                    GPS_dataup();
                    
                }    
            }
            // updata the previous distance
            dis_last = target_distance;     
            
            // updata the previous Aoa_former
            Aoa_former_last = Aoa_former;   
        }
        else             
        {
#if 0
            // Cycle run
            GPSData->i = GPS_INITIAL_POINT;  
#elif 1
            //Stop at the finish line
            V_duty = 0;       
#endif
        }
    }
}

//-------------------------------------------------------------------------------------------------------------------
// @brief		    
// @param		    
// @return		              
// @since		    v1.0
// explain          distance 
// Sample usage:
//-------------------------------------------------------------------------------------------------------------------
void Encoder_Timer(void) 
{
    if(gpio_get_level(ENCODER_DIR) == 0)
    {
        encoder = - encoder_get_count(ENCODER_TIM);        // 采集对应编码器数据
    }else
    {
        encoder = encoder_get_count(ENCODER_TIM);        // 采集对应编码器数据
    }
    encoder_clear_count(ENCODER_TIM);                   // 清除对应计数
    
    get_speed_sum = encoder* 0.9 + V_old * 0.1;
    
    meter_second = (float)get_speed_sum * MeterSecond * TimeConsersion; 
    //                                    0.276 
    hall_dis += meter_second / 84.315f * 0.276*2;//0.2m -- 2.1m                
    
    V_old = get_speed_sum;
}

//-------------------------------------------------------------------------------------------------------------------
// @brief		     
// @param		     
// @return		     
// @since		    v1.0
// Sample usage:    GPS_inf_reset(&GPS_Data);
// describe:         
//-------------------------------------------------------------------------------------------------------------------
void GPS_inf_reset(void)
{
    for(GPS_Data.i = 0; GPS_Data.i < 100; GPS_Data.i++ ) 
    {
        GPS_Data.lat[GPS_Data.i] = 0;GPS_Data.lon[GPS_Data.i] = 0;
    }
    GPS_Data.i = 0;
    target_distance = 0;
    target_direction = 0;
    // ring(50);
}
//-------------------------------------------------------------------------------------------------------------------
// @brief		    initialization of GPS information collection
// @param		    GPS structure
// @return		    
// @since		    v1.0
// Sample usage:    GPS_inf_init();
// describe:        
//-------------------------------------------------------------------------------------------------------------------
void GPS_inf_init(void)
{   
    // If the FLASH has GPS data, it reads the GPS data in the Flash
    if(flash_check(FLASH_SECTION_GPS, FLASH_PAGE_LAT))
    {
        get_flash_gps(FLASH_SECTION_GPS, FLASH_PAGE_LAT, FLASH_PAGE_LON);	
    }
    
    // Calculate the distance and azimuth parameters of coordinates
    for(GPS_Data.i = 2; GPS_Data.i < 200 && GPS_Data.lat[GPS_Data.i] != 0; GPS_Data.i++ )  
    {
        
        GPS_Data.former_i_dir[GPS_Data.i] = get_two_points_azimuth(GPS_Data.lat[GPS_Data.i-1], GPS_Data.lon[GPS_Data.i-1], GPS_Data.lat[GPS_Data.i], GPS_Data.lon[GPS_Data.i]);
        GPS_Data.former_i_dis[GPS_Data.i] = get_two_points_distance(GPS_Data.lat[GPS_Data.i-1], GPS_Data.lon[GPS_Data.i-1], GPS_Data.lat[GPS_Data.i], GPS_Data.lon[GPS_Data.i]); 
        
        if(GPS_Data.i > 3)
        {
            GPS_Data.former_i_Aoa[GPS_Data.i] = GPS_Data.former_i_dir[GPS_Data.i-1] - GPS_Data.former_i_dir[GPS_Data.i] ;
            
        }
        
    }
    // Determine which points are in the straight
    
    gps_tau1201.GPS_new_data_exe = 2;
    
    GPS_Data.i = GPS_INITIAL_POINT;
    target_distance = 0;target_direction = 0;
    
}
//-------------------------------------------------------------------------------------------------------------------
// @brief		    GPS data updata
// @param		    
// @return		    Non
// @since		    v1.0
// Sample usage:    menu_dataup();		
//-------------------------------------------------------------------------------------------------------------------
/*
我在每次得到一次新的GPS数据时
都会计算当前GPS坐标点 与 目标点i 的距离 已经 方位角
以及 与 目标点i+1 的距离 以及方位角
再计算一个角度差 Aoa_tar 
*/
void GPS_dataup()
{
    if(GPS_Data.i > 0 && GPS_Data.lat[GPS_Data.i] != 0)
    {   //
        target_last_dis = get_two_points_distance(gps_tau1201.latitude, gps_tau1201.longitude, GPS_Data.lat[GPS_Data.i-1], GPS_Data.lon[GPS_Data.i-1]);
        
        target_distance = get_two_points_distance(gps_tau1201.latitude, gps_tau1201.longitude, GPS_Data.lat[GPS_Data.i], GPS_Data.lon[GPS_Data.i]);
        target_direction = get_two_points_azimuth(gps_tau1201.latitude, gps_tau1201.longitude, GPS_Data.lat[GPS_Data.i], GPS_Data.lon[GPS_Data.i]);
        
        target_next_dis = get_two_points_distance(gps_tau1201.latitude, gps_tau1201.longitude, GPS_Data.lat[GPS_Data.i+1], GPS_Data.lon[GPS_Data.i+1]);
        target_next_dir = get_two_points_azimuth(gps_tau1201.latitude, gps_tau1201.longitude, GPS_Data.lat[GPS_Data.i+1], GPS_Data.lon[GPS_Data.i+1]);
    }else if(GPS_Data.lat[GPS_Data.i] == 0)
    {
        target_distance = 0;target_direction = 0;
    }
    // 当前角度差 = 角度限制（车头方位角 - 与目标点方位角）
    Aoa_tar = Angle_limiter(gps_tau1201.direction - target_direction); // limit aoa_tar in -180 ~ +180
    
    // 角度差 = 车头方位角 - i点与i-1的方位角
    Aoa_former = Angle_limiter(gps_tau1201.direction - GPS_Data.former_i_dir[GPS_Data.i]);
    
}
//-------------------------------------------------------------------------------------------------------------------
// @brief		    GPS data recalculate
// @param		    The current i
// @return		    Non
// @since		    v1.0
// Sample usage:    GPS_recalculate(GPS_Data.i);	
//-------------------------------------------------------------------------------------------------------------------
void GPS_recalculate(int16 current_i)
{
    int16 temporary_i = current_i;
    // recalculate the data adjacent to the current point
    for(temporary_i = (current_i-2); temporary_i < (current_i+2) && GPS_Data.lat[temporary_i] != 0; temporary_i++ )  
    {
        GPS_Data.former_i_dis[temporary_i] = get_two_points_distance(GPS_Data.lat[temporary_i-1], GPS_Data.lon[temporary_i-1], GPS_Data.lat[temporary_i], GPS_Data.lon[temporary_i]); 
        GPS_Data.former_i_dir[temporary_i] = get_two_points_azimuth(GPS_Data.lat[temporary_i-1], GPS_Data.lon[temporary_i-1], GPS_Data.lat[temporary_i], GPS_Data.lon[temporary_i]) ; 
        GPS_Data.former_i_Aoa[temporary_i] = GPS_Data.former_i_dir[temporary_i-1] - GPS_Data.former_i_dir[temporary_i] ;
    }
}
//-------------------------------------------------------------------------------------------------------------------
// @brief		    Read GPS data from flash memory
// @param		    
// @return		    Non
// @since		    v1.0
// Sample usage:    get_flash_gps(FLASH_SECTION_GPS, FLASH_PAGE_LAT, FLASH_PAGE_LON);	
//-------------------------------------------------------------------------------------------------------------------
void get_flash_gps(flash_section_enum flash_gps, flash_page_enum page_lat, flash_page_enum page_lon)
{
    vint16 test_i = 1;
    if(flash_check(flash_gps, page_lat))                                    // page_lat there is data; 1- has data 0- Has no data
    {
        flash_buffer_clear();                                               // Clear buffer
        
        flash_read_page_to_buffer(flash_gps, page_lat);                         // Read data from flash into the buffer
        while (flash_union_buffer[test_i].float_type > 0 && test_i < 200)       // Cache array current value exists
        {
            GPS_Data.lat[test_i] = flash_union_buffer[test_i].float_type;       // assignment
            test_i++;                                                           // Point to the next
        }
    }
    
    if(flash_check(flash_gps, page_lon))                                    // page_lon there is data; 1- has data 0- Has no data
    {
        flash_buffer_clear();                                               // Clear buffer
        test_i = 1;
        flash_read_page_to_buffer(flash_gps, page_lon);                         // Read data from flash into the buffer
        while (flash_union_buffer[test_i].float_type > 0 && test_i < 200)       // Cache array current value exists
        {
            GPS_Data.lon[test_i] = flash_union_buffer[test_i].float_type;       // assignment
            test_i++;                                                           // Point to the next
        }
    }
    if(GPS_Data.lon[test_i-1] > 0 && GPS_Data.lat[test_i-1] > 0)
        ring(100);                                                          // Finished mark
}

//-------------------------------------------------------------------------------------------------------------------
// @brief		    Store GPS information in flash memory
// @param		    
// @return		    Non
// @since		    v1.0
// Sample usage:    store_gps_flash(FLASH_SECTION_GPS, FLASH_PAGE_LAT, FLASH_PAGE_LON);	
//-------------------------------------------------------------------------------------------------------------------
void store_gps_flash(flash_section_enum flash_gps, flash_page_enum page_lat, flash_page_enum page_lon)
{
    vint16 test_i = 1;
    flash_buffer_clear();                                                    // Clear buffer
    while(GPS_Data.lat[test_i] > 0 && test_i < 200)                          // Start storing latitude
    {
        flash_union_buffer[test_i].float_type  = GPS_Data.lat[test_i];               
        test_i++;
    }
    if(!flash_write_page_from_buffer(flash_gps, page_lat))                   // Successfully saved latitude information; 1- failure. 0- success
    {
        flash_buffer_clear();                                                // Clear buffer
        test_i = 1;
        while(GPS_Data.lon[test_i] > 0 && test_i < 200)                      //Start storing longitude
        {
            flash_union_buffer[test_i].float_type  = GPS_Data.lon[test_i];  
            test_i++;
        }
        if(!flash_write_page_from_buffer(flash_gps, page_lon)) 
            ring(200);                                                       // Store completion flag
    }
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief         
//  @author         -350 -170
//  data         
//  @return       float
//  @since        v1.0
//  Explain:      Angle_limiter(ANGLE);//0~360 ==> +-180
//-------------------------------------------------------------------------------------------------------------------
float Angle_limiter(float ANGLE)
{
    if(ANGLE > 180) ANGLE -= 360;
    else if(ANGLE < -180) ANGLE += 360;
    return ANGLE;
}
//-------------------------------------------------------------------------------------------------------------------
//  @brief         
//  @author        
//  data          
//  @return       float
//  @since        v1.0
//  Explain:       
//-------------------------------------------------------------------------------------------------------------------
float Loca_PID(PID* Loca, float Expect, float Actual)
{
	Loca->Error = Expect - Actual;
    
    Loca->Error = Angle_limiter(Loca->Error);
    
	Loca->Error_ADD += Loca->Error; 
    
    if(Loca->Limit)
        limit(Loca->I * Loca->Error_ADD, Loca->Limit); 
    
	Loca->Error_Rate = Loca->Error - Loca->Error_pre; 
    
	Loca->Output = (Loca->P * Loca->Error) + (Loca->I * Loca->Error_ADD) + (Loca->D * Loca->Error_Rate);
    
	Loca->Error_pre = Loca->Error; 
    
    if(Loca->mid_value)
    {
#if Servo_SD9
        Loca->Output = Loca->mid_value + Loca->Output;
#elif !Servo_SD9
        Loca->Output = Loca->mid_value - Loca->Output;
#endif
    }
	return Loca->Output;
}

//-------------------------------------------------------------------------------------------------------------------
//  brief          
//  author         
//  data          
//  return        float
//  since         v1.0
//  Sample usage:
//
//-------------------------------------------------------------------------------------------------------------------
float Incre_PID(PID* Incre, int16 Expect, int16 Actual)
{
	Incre->Error = Expect - Actual; 
    
	Incre->Error_Rate = Incre->Error - Incre->Error_pre; 
    
	Incre->Error_Ratepro = Incre->Error - 2 * Incre->Error_pre + Incre->Error_prepre; 
    
	Incre->Output += (int)(Incre->P * Incre->Error_Rate) + (Incre->I * Incre->Error) + (Incre->D * Incre->Error_Ratepro);
    
	Incre->Error_prepre = Incre->Error_pre; 
    
	Incre->Error_pre = Incre->Error; 
    
	Incre->Output = (int16)limit(Incre->Output, Incre->Limit); 
    
	return Incre->Output;
}




